#include "motor.h"
#include "gpio.h"
#include "pid.h"

#define zq_right_RESET   HAL_GPIO_WritePin(GPIOG,GPIO_PIN_0,GPIO_PIN_RESET); 
#define zq_right_SET     HAL_GPIO_WritePin(GPIOG,GPIO_PIN_1,GPIO_PIN_SET); 
#define zq_left_SET 	 	 HAL_GPIO_WritePin(GPIOG,GPIO_PIN_2,GPIO_PIN_SET);
#define zq_left_RESET    HAL_GPIO_WritePin(GPIOG,GPIO_PIN_3,GPIO_PIN_RESET); 

#define zh_right_RESET   HAL_GPIO_WritePin(GPIOF,GPIO_PIN_14,GPIO_PIN_RESET); 
#define zh_right_SET     HAL_GPIO_WritePin(GPIOF,GPIO_PIN_15,GPIO_PIN_SET); 
#define zh_left_SET 	 	 HAL_GPIO_WritePin(GPIOG,GPIO_PIN_4,GPIO_PIN_SET);
#define zh_left_RESET    HAL_GPIO_WritePin(GPIOG,GPIO_PIN_5,GPIO_PIN_RESET);
/*反转*/
#define fq_right_SET     HAL_GPIO_WritePin(GPIOG,GPIO_PIN_0,GPIO_PIN_SET); 
#define fq_right_RESET   HAL_GPIO_WritePin(GPIOG,GPIO_PIN_1,GPIO_PIN_RESET); 
#define fq_left_RESET 	 HAL_GPIO_WritePin(GPIOG,GPIO_PIN_2,GPIO_PIN_RESET);
#define fq_left_SET      HAL_GPIO_WritePin(GPIOG,GPIO_PIN_3,GPIO_PIN_SET); 

#define fh_right_SET     HAL_GPIO_WritePin(GPIOF,GPIO_PIN_14,GPIO_PIN_SET); 
#define fh_right_RESET   HAL_GPIO_WritePin(GPIOF,GPIO_PIN_15,GPIO_PIN_RESET); 
#define fh_left_RESET 	 HAL_GPIO_WritePin(GPIOG,GPIO_PIN_4,GPIO_PIN_RESET);
#define fh_left_SET      HAL_GPIO_WritePin(GPIOG,GPIO_PIN_5,GPIO_PIN_SET);

extern TIM_HandleTypeDef htim1;

extern tPid pidMotor1Speed;
extern tPid pidMotor2Speed;
extern tPid pidMotor3Speed;
extern tPid pidMotor4Speed;

extern float Motor1Speed;
extern float Motor2Speed;
extern float Motor3Speed;
extern float Motor4Speed;

extern int16_t PI_data[2];//0-左轮速度 1-右轮速度

void car_init(void)
{
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
}

void motor_set(int motor1,int motor2,int motor3,int motor4)
{
	if(motor1 < 0)
	{
		zq_right_RESET;
		zq_right_SET;
	}
	else
	{
		fq_right_SET;
		fq_right_RESET;
	}
	if(motor2 < 0)
	{
    zq_left_RESET;
		zq_left_SET;	
	}
	else
	{
		fq_left_SET;
		fq_left_RESET;
	}
	if(motor4 < 0)
	{
    fh_right_SET;
		fh_right_RESET;
	}
	else
	{
    zh_right_RESET;
		zh_right_SET;
	}
	if(motor3 < 0)
	{
    fh_left_SET;
		fh_left_RESET;
	}
	else
	{
    zh_left_RESET;
		zh_left_SET;  
	}
	if(motor1<0)
	{
		if(motor1<-99) motor1=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (100+motor1));//修改定时器1

	}
	else
	{
		if(motor1>99) motor1=99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor1);//修改定时器1
	}
	if(motor2<0)
	{
		if(motor2<-99) motor2=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (100+motor2));//修改定时器1

	}
	else
	{
		if(motor2>99) motor2=99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, motor2);//修改定时器1
	}
	if(motor3<0)
	{
		if(motor3<-99) motor3=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (100+motor3));//修改定时器1

	}
	else
	{
		if(motor3>99) motor3=99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, motor3);//修改定时器1
	}
	if(motor4<0)
	{
		if(motor4<-99) motor4=-99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, (100+motor4));//修改定时器1

	}
	else
	{
		if(motor4>99) motor4=99;
		__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, motor4);//修改定时器1
	}
}

//void motorPidSetSpeed(float Motor1SetSpeed,float Motor2SetSpeed,float Motor3SetSpeed,float Motor4SetSpeed)
//{
////改变电机PID参数的目标速度
//    pidMotor1Speed.target_val = Motor1SetSpeed;
//    pidMotor2Speed.target_val = Motor2SetSpeed;
//    pidMotor3Speed.target_val = Motor3SetSpeed;
//    pidMotor4Speed.target_val = Motor4SetSpeed;
////根据PID计算 输出作用于电机
//    motor_set(PID_realize(&pidMotor1Speed,Motor1Speed),PID_realize(&pidMotor2Speed,Motor2Speed),
//              PID_realize(&pidMotor3Speed,Motor3Speed),PID_realize(&pidMotor4Speed,Motor4Speed));
//}
void Car_Proc(void)
{
	motor_set(PI_data[0],PI_data[1],PI_data[0],PI_data[1]);
}
